import rospy
from std_msgs.msg import Float32

M_PI = (3.1415926/3.0)
class ROSVRXInfo:
    def __init__(self, can_name, propeller_mid, servo_mid):
        self.last_propeller = 0
        self.last_servo = 0
        self.propeller_mid = propeller_mid
        self.servo_mid = servo_mid
        self.propeller_k = (1 - (-1)) / (100.0 - (-100.0))
        self.servo_k = M_PI / (100.0 - (-100.0))

        self.node = rospy.init_node('rosvrx_info')
        self.left_angle = rospy.Publisher('/wamv/thrusters/left_thrust_angle', Float32, queue_size=1)
        self.left_cmd = rospy.Publisher('/wamv/thrusters/left_thrust_cmd', Float32, queue_size=1)
        self.right_angle = rospy.Publisher('/wamv/thrusters/right_thrust_angle', Float32, queue_size=1)
        self.right_cmd = rospy.Publisher('/wamv/thrusters/right_thrust_cmd', Float32, queue_size=1)

    def get_trans_propeller(self, propeller):
        propeller_trans = 0
        if propeller >= 100:
            propeller_trans = 1
        elif propeller <= -100:
            propeller_trans = -1
        elif propeller == 0:
            propeller_trans = self.propeller_mid
        else:
            propeller_trans = self.propeller_k * (propeller + self.propeller_mid)

        if propeller_trans > 1:
            propeller_trans = 1
        elif propeller_trans < -1:
            propeller_trans = -1

        return propeller_trans

    def get_trans_servo(self, servo):
        servo_trans = 0
        if servo >= 100:
            servo_trans = M_PI / 2
        elif servo <= -100:
            servo_trans = -M_PI / 2
        else:
            servo_trans = self.servo_k * (servo + self.servo_mid)

        if servo_trans > M_PI / 2:
            servo_trans = M_PI / 2
        elif servo_trans < -M_PI / 2:
            servo_trans = -M_PI / 2

        return servo_trans

    def manual_act(self, propeller, servo):
        power = self.get_trans_propeller(propeller)
        turn = self.get_trans_servo(servo)

        self.last_propeller = propeller
        self.last_servo = servo

        f32_value = Float32()
        f32_value.data = power
        self.left_cmd.publish(f32_value)
        self.right_cmd.publish(f32_value)

        f32_value.data = turn
        self.left_angle.publish(f32_value)
        self.right_angle.publish(f32_value)


